package com.hit.client.driver;

import com.alibaba.fastjson.JSONObject;
import com.hit.client.api.dto.config.RobotClientProperties;
import com.hit.client.driver.dto.*;
import com.hit.client.driver.enums.RobotOtherCommandEnum;
import com.hit.client.driver.util.GfCacheUtil;
import com.hit.client.driver.util.GfToolkit;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Value;
import org.springframework.stereotype.Service;

import java.text.DecimalFormat;
import java.util.*;
import java.util.function.Function;


@Slf4j
@RequiredArgsConstructor
@Service("robotDriverService")
public class HitGfRobotDriverImpl implements RobotDriverService {
    //驱动电机web控制速度 r/min
    private static short defaultSpeed = 800;
    // r/min
    private static final short minSpeed = 500;
    // r/min
    private static final short maxSpeed = 1700;

    private static Map<String, Short> speedMap = new HashMap<>();

    private final RobotClientProperties clientProperties;

    @Override
    public boolean init() {
        boolean result = false;
        try {
            result = GfToolkit.init(clientProperties.getBaseInfo().getClientTcpPort());
        } catch (Exception e) {
            log.error("[gf] init error", e);


        }
        log.info("[gf] init success");
        return result;
    }

    @Override
    public boolean setSpeed(String serialNumber, Integer oprType, Integer direction) {
        Short speed = speedMap.getOrDefault(serialNumber, defaultSpeed);
        if (oprType == 1) {//机器人
            GfCacheUtil.setSpeedFlag(serialNumber);
            GfToolkit.setMotorSpeed(serialNumber, direction * speed, 0);
            return true;
        } else if (oprType == 2) {//换行车
            GfCacheUtil.setCarSpeedFlag(serialNumber);
            GfToolkit.setMotorSpeedOfCar(serialNumber, direction * speed);
            return true;
        }
        return false;
    }

    @Override
    public boolean setSpeedUpOrDown(String serialNumber, Integer step) {
        Short speed = speedMap.getOrDefault(serialNumber, defaultSpeed);
        speed = (short) Math.min(Math.max(speed + step, minSpeed), maxSpeed);
        speedMap.put(serialNumber, speed);
        return true;
    }


    @Override
    public boolean motionAbort(String serialNumber, Integer oprType) {
        return this.setSpeed(serialNumber, oprType, 0);
    }

    @Override
    @SuppressWarnings("unchecked")
    public boolean executeOtherCommand(String serialNumber, RobotOtherCommandEnum cmdEnum, String jsonParams) {
        switch (cmdEnum) {
            case SET_WORK_MODE -> {
                Map<String, Object> params = JSONObject.parseObject(jsonParams, Map.class);
                return GfToolkit.setWork(serialNumber,
                        (byte) Integer.parseInt(String.valueOf(params.get("workMode"))),
                        Integer.parseInt(String.valueOf(params.get("qdMotorSpeed"))),
                        Integer.parseInt(String.valueOf(params.get("gsMotorSpeed"))),
                        (byte) Integer.parseInt(String.valueOf(params.get("workNum")))
                );
            }
            case SET_GF_FUNCTION -> {
                Map<String, Object> params = JSONObject.parseObject(jsonParams, Map.class);
                return GfToolkit.setFunctionEnable(serialNumber,
                        (byte) Integer.parseInt(String.valueOf(params.get("function"))),
                        (byte) Integer.parseInt(String.valueOf(params.get("enable")))
                );
            }
            case SET_GF_IO -> {
                Map<String, Object> params = JSONObject.parseObject(jsonParams, Map.class);
                return GfToolkit.setIoEnable(serialNumber,
                        (byte) Integer.parseInt(String.valueOf(params.get("number"))),
                        (byte) Integer.parseInt(String.valueOf(params.get("enable")))
                );
            }
            case SET_GF_RELAY -> {
                Map<String, Object> params = JSONObject.parseObject(jsonParams, Map.class);
                return GfToolkit.setRelayStatus(serialNumber,
                        (byte) Integer.parseInt(String.valueOf(params.get("number"))),
                        (byte) Integer.parseInt(String.valueOf(params.get("enable")))
                );
            }
            case SET_CAR_RELAY -> {
                Map<String, Object> params = JSONObject.parseObject(jsonParams, Map.class);
                return GfToolkit.setRelayStatusOfCar(serialNumber,
                        (byte) Integer.parseInt(String.valueOf(params.get("number"))),
                        (byte) Integer.parseInt(String.valueOf(params.get("enable")))
                );
            }
            case SET_CAR_ENABLED -> {
                Map<String, Object> params = JSONObject.parseObject(jsonParams, Map.class);
                return GfToolkit.setMotorEnabledOfCar(serialNumber, (byte) Integer.parseInt(String.valueOf(params.get("enable"))));
            }
            case SET_CAR_GO_POS -> {
                Map<String, Object> params = JSONObject.parseObject(jsonParams, Map.class);
                return GfToolkit.setCarMoveToPos(serialNumber, (byte) Integer.parseInt(String.valueOf(params.get("posId"))));
            }
            case SET_CAR_PUSH -> {
                Map<String, Object> params = JSONObject.parseObject(jsonParams, Map.class);
                return GfToolkit.setPushStatusOfCar(serialNumber, (byte) Integer.parseInt(String.valueOf(params.get("ctrlType"))));
            }
            case SET_CAR_TASK -> {
                Map<String, Object> params = JSONObject.parseObject(jsonParams, Map.class);
                return GfToolkit.setTaskStatusOfCar(serialNumber, (byte) Integer.parseInt(String.valueOf(params.get("ctrlType"))));
            }
        }
        return false;
    }

    @Override
    public RobotInfoDto getRobotInfo(String serialNumber) {
        RobotInfoDto robotInfoDto = new RobotInfoDto();
        GfRelayStatus relayStatus = (GfRelayStatus) GfCacheUtil.getLongtimeCache(serialNumber, GfCacheUtil.KEY_ROBOT_STATUS);
        if (relayStatus != null) {
            robotInfoDto.setCurrentDirection("待机");
        }
        GfRobotStatus status = (GfRobotStatus) GfCacheUtil.get(serialNumber, GfCacheUtil.KEY_ROBOT_STATUS);
        if (status != null) {
            robotInfoDto.setControllerTemp(status.getChipTemperature());
            robotInfoDto.setBatteryLevel(status.getPower());
            robotInfoDto.setCharging(status.getCurrentDirection() == 1);
            robotInfoDto.setVoltage(status.getVoltage());
            robotInfoDto.setCurrent(String.valueOf(status.getCurrent()));
            DecimalFormat df = new DecimalFormat("0.###");
            robotInfoDto.setSpeed(Double.parseDouble(df.format(status.getRobotSpeed())));
            robotInfoDto.setCurrentDirection(status.getCurrentDirection() == 0 ? "放电" : "充电");
            robotInfoDto.setTemperature(status.getTemperature());
            robotInfoDto.setMileage(status.getMileage());
        }
        return robotInfoDto;
    }

    @Override
    public CarInfoDto getCarInfo(String serialNumber) {
        CarInfoDto carInfoDto = new CarInfoDto();
        GfRobotStatusOfCar status = (GfRobotStatusOfCar) GfCacheUtil.get(serialNumber, GfCacheUtil.KEY_ROBOT_STATUS_OF_CAR);
        if (status != null) {
            carInfoDto.setBatteryLevel(status.getPower());
            carInfoDto.setVoltage(status.getQdlMotorVoltage());
            carInfoDto.setCurrent(String.valueOf(status.getQdlMotorCurrent()));
            DecimalFormat df = new DecimalFormat("0.###");
            carInfoDto.setSpeed(Double.parseDouble(df.format(status.getQdlSpeed())));
            carInfoDto.setTemperature(status.getQdlMotorTemperature());
        }
        return carInfoDto;
    }


    @Override
    public List<CustomInfo> getRobotCustomInfo(String serialNumber) {
        List<CustomInfo> customInfo = new ArrayList<CustomInfo>();
        Object gfStatus = GfCacheUtil.get(serialNumber, GfCacheUtil.KEY_ROBOT_STATUS);
        GfRobotStatus status = GfRobotStatus.builder().robotSerial("").reachStatus((short) 0).qdlSpeed(0)
                .gsSpeed(0).qdlMotorCurrent(0).gsMotorCurrent(0)
                .qdlMotorDis(0).gsMotorDis(0).qdlMotorErr(0).gsMotorErr(0)
                .power((short) 0).voltage(0).current(0).ratedCapacity(0)
                .temperature((short) 0).currentDirection((short) 0).directionInfo((short) 0)
                .workNum((byte) 0).workStatus((byte) 0).psStatus((byte) 0).psErrStatus((byte) 0)
                .chipTemperature((byte) 0).psRunTime(0).psOpenStatus((byte) 0).forwardStatus((byte) 0)
                .psSetTime(0).leftProximitySwitchTriggeringStatus((byte) 0)
                .rightProximitySwitchTriggeringStatus((byte) 0).robotSpeed(0)
                .reverseStatus((byte) 0).cameraAndWiperEnable((byte) 0).runningDriverEnable((byte) 0)
                .gsDriverEnable((byte) 0).ipcEnable((byte) 0).build();
        if (gfStatus != null) {
            status = (GfRobotStatus) gfStatus;
        }
        GfRelayStatus relayStatus = (GfRelayStatus) GfCacheUtil.getLongtimeCache(serialNumber, GfCacheUtil.KEY_ROBOT_STATUS);
        customInfo.add(CustomInfo.create("relayStatus1", "继电器1状态", relayStatus != null && relayStatus.getStatus1() == 1 ? "开启" : "关闭"));
        customInfo.add(CustomInfo.create("relayStatus2", "继电器2状态", relayStatus != null && relayStatus.getStatus2() == 1 ? "开启" : "关闭"));
        customInfo.add(CustomInfo.create("qdlSpeed", "驱动轮速度", status.getQdlSpeed() + "(R/min)"));
        customInfo.add(CustomInfo.create("gsSpeed", "滚刷速度", status.getGsSpeed() + "(R/min)"));
        customInfo.add(CustomInfo.create("qdlMotorCurrent", "驱动轮电机电流", status.getQdlMotorCurrent() + "(A)"));
        customInfo.add(CustomInfo.create("gsMotorCurrent", "滚刷电机电流", status.getGsMotorCurrent() + "(A)"));
        customInfo.add(CustomInfo.create("qdlMotorDis", "驱动轮电机里程计", String.valueOf(status.getQdlMotorDis())));
        customInfo.add(CustomInfo.create("gsMotorDis", "滚刷电机里程计", String.valueOf(status.getGsMotorDis())));
        customInfo.add(CustomInfo.create("qdlMotorErr", "驱动轮电机故障状态", status.getQdlMotorErr() == 0 ? "正常" : "故障码:" + status.getQdlMotorErr()));
        customInfo.add(CustomInfo.create("gsMotorErr", "滚刷电机故障状态", status.getQdlMotorErr() == 0 ? "正常" : "故障码:" + status.getGsMotorErr()));
        customInfo.add(CustomInfo.create("workNum", "当前工作次数", String.valueOf(status.getWorkNum())));
        customInfo.add(CustomInfo.create("chipTemperature", "芯片温度", String.valueOf(status.getChipTemperature())));
        customInfo.add(CustomInfo.create("psStatus", "光电传感器状态", status.getPsStatus() == 0 ? "未触发" : "触发"));
        customInfo.add(CustomInfo.create("psErrStatus", "光电传感器堵转状态", status.getPsErrStatus() == 0 ? "未触发" : "触发"));
        customInfo.add(CustomInfo.create("psOpenStatus", "光电避障开关状态", status.getPsOpenStatus() == 1 ? "开启" : "关闭"));
        customInfo.add(CustomInfo.create("leftProximitySwitchTriggeringStatus", "左接近开关状态", status.getLeftProximitySwitchTriggeringStatus() == 0 ? "触发" : "未触发"));
        customInfo.add(CustomInfo.create("rightProximitySwitchTriggeringStatus", "右接近开关状态", status.getRightProximitySwitchTriggeringStatus() == 0 ? "触发" : "未触发"));
        customInfo.add(CustomInfo.create("cameraAndWiperEnable", "雨刷与相机使能状态", status.getCameraAndWiperEnable() == 1 ? "使能" : "未使能"));
        customInfo.add(CustomInfo.create("runningDriverEnable", "行走电机使能状态", status.getRunningDriverEnable() == 1 ? "使能" : "未使能"));
        customInfo.add(CustomInfo.create("gsDriverEnable", "滚刷电机使能状态", status.getGsDriverEnable() == 1 ? "使能" : "未使能"));
        customInfo.add(CustomInfo.create("ipcEnable", "工控机使能状态", status.getIpcEnable() == 1 ? "使能" : "未使能"));
        customInfo.add(CustomInfo.create("mileage", "编码器编码值", String.valueOf(status.getMileage())));
        return customInfo;
    }

    @Override
    public List<CustomInfo> getCarCustomInfo(String serialNumber) {
        List<CustomInfo> customInfo = new ArrayList<CustomInfo>();
        Object gfStatus = GfCacheUtil.get(serialNumber, GfCacheUtil.KEY_ROBOT_STATUS_OF_CAR);
        GfRobotStatusOfCar status = GfRobotStatusOfCar.builder()
                .robotSerial("").reachStatus((short) 0).qdlSpeed((short) 0)
                .qdlMotorDis(0).qdlMotorCurrent(0).qdlMotorVoltage(0)
                .qdlMotorTemperature((short) 0).bmqDis(0).zkbVoltage(0).dcVoltage(0)
                .workStatus((byte) 0).robotWorkFlowStatus((byte) 0).carWorkFlowStatus((byte) 0)
                .carLocationStatus((byte) 0).carPositionStatus((byte) 0).driverEnable((byte) 0)
                .carTaskStatus((byte) 0).power((short) 0)
                .build();
        if (gfStatus != null) {
            status = (GfRobotStatusOfCar) gfStatus;
        }
        GfRelayStatus relayStatus = (GfRelayStatus) GfCacheUtil.getLongtimeCache(serialNumber, GfCacheUtil.KEY_ROBOT_STATUS_OF_CAR);
        customInfo.add(CustomInfo.create("relayStatus1", "继电器1状态", relayStatus != null && relayStatus.getStatus1() == 1 ? "开启" : "关闭"));
        customInfo.add(CustomInfo.create("relayStatus2", "继电器2状态", relayStatus != null && relayStatus.getStatus2() == 1 ? "开启" : "关闭"));
        customInfo.add(CustomInfo.create("sn", "换行车序列号", status.getRobotSerial()));
        customInfo.add(CustomInfo.create("qdlSpeed", "驱动轮速度", status.getQdlSpeed() + "(R/min)"));
        customInfo.add(CustomInfo.create("qdlMotorDis", "驱动轮电机里程计", String.valueOf(status.getQdlMotorDis())));
        customInfo.add(CustomInfo.create("qdlMotorCurrent", "驱动轮电机电流（A）", status.getQdlMotorCurrent() + "(A)"));
        customInfo.add(CustomInfo.create("qdlMotorVoltage", "驱动轮电机电压（V）", status.getQdlMotorVoltage() + "(V)"));
        customInfo.add(CustomInfo.create("qdlMotorTemperature", "驱动轮电机温度", status.getQdlMotorTemperature() + "(℃)"));
        customInfo.add(CustomInfo.create("bmqDis", "编码器里程计", String.valueOf(status.getBmqDis())));
        customInfo.add(CustomInfo.create("zkbVoltage", "主控板电压（V）", status.getZkbVoltage() + "(V)"));
        customInfo.add(CustomInfo.create("dcVoltage", "电池电压（V）", status.getDcVoltage() + "(V)"));
        Function<Byte, String> convertWorkStatus = (workStatus) -> {
            switch (workStatus) {
                case 0x00:
                    return "机器空闲";
                case 0x01:
                    return "机器工作";
                case 0x10:
                    return "电机堵转";
                case 0x11:
                    return "通讯异常";
                case 0x12:
                    return "电量过低";
                case 0x20:
                    return "进入程序状态";
                case 0x21:
                    return "程序升级中";
                case 0x22:
                    return "程序升级完成";
                default:
                    return "未知";
            }
        };
        customInfo.add(CustomInfo.create("workStatus", "机器人工作状态", convertWorkStatus.apply(status.getWorkStatus())));
        customInfo.add(CustomInfo.create("robotWorkFlowStatus", "光伏机器人工作流程状态", status.getRobotWorkFlowStatus() == 0x10 ? "光伏机器人就位" : "光伏机器人离开"));
        Function<Byte, String> convertCarWorkFlowStatus = (carWorkFlowStatus) -> {
            switch (carWorkFlowStatus) {
                case 0x00:
                    return "机器人、驱动轮插销解锁";
                case 0x01:
                    return "机器人插销解锁，驱动轮插销锁死";
                case 0x10:
                    return "机器人插销锁死，驱动轮插销解锁";
                case 0x11:
                    return "机器人插销锁死，驱动轮插销锁死";
                default:
                    return "未知";
            }
        };
        customInfo.add(CustomInfo.create("carWorkFlowStatus", "换行车工作流程状态", convertCarWorkFlowStatus.apply(status.getCarWorkFlowStatus())));
        customInfo.add(CustomInfo.create("carLocationStatus", "换行车位置状态", status.getCarLocationStatus() == 0x00 ? "处于起点" : "处于终点"));
        customInfo.add(CustomInfo.create("carPositionStatus", "换行车点位状态", String.valueOf(status.getCarPositionStatus())));
        customInfo.add(CustomInfo.create("driverEnable", "电机使能状态", status.getDriverEnable() == 0x00 ? "未使能" : "使能"));
        Function<Byte, String> convertCarTaskStatus = (carTaskStatus) -> {
            switch (carTaskStatus) {
                case 0x00:
                    return "换行车空闲";
                case 0x01:
                    return "换行车自动任务";
                case 0x02:
                    return "交由上位机控制";
                default:
                    return "未知";
            }
        };
        customInfo.add(CustomInfo.create("carTaskStatus", "换行车任务状态", convertCarTaskStatus.apply(status.getCarTaskStatus())));
        customInfo.add(CustomInfo.create("power", "电池电量", status.getPower() + "(%)"));
        return customInfo;
    }

}
